// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ImageOperations2DTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Vision
{
    using System;
    using System.Collections.Generic;
    using System.Drawing;
    using System.IO;
    using System.Threading;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.ImageProcessing2D;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Test for ImageOperations2D and ImageOperations2DInterop methods
    /// </summary>
    [TestClass]
    public class ImageOperations2DTest
    {
        /// <summary>
        /// Validation of load image method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void LoadImageRgbFromFileShouldLoadProperly()
        {
            string filename = @"\\robofile\Baker\Shared\Users\roboim\UnitTestData\Vision\ImageFrameTest.png";
            ImageFrameRgb image = ImageOperations2DInterop.LoadImageRgbFromFile(filename);

            // check the values of some arbitrary pixels
            Assert.AreEqual(image[0, 0], new RgbPix(120, 120, 120));
            Assert.AreEqual(image[201, 153], new RgbPix(132, 132, 132));
            Assert.AreEqual(image[53, 308], new RgbPix(61, 61, 61));
            Assert.AreEqual(image[514, 239], new RgbPix(92, 92, 92));
        }

        /// <summary>
        /// Validation of bilateral filter on image of all zeros
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void BilateralFilterShouldWorkForImageOfAllZeros()
        {
            const double SpatialSigma = 1.0;
            const double RangeSigma = 1.0;

            // Initialize image
            ImageFrameDepth image = new ImageFrameDepth(20, 20);

            // Perform bilateral filtering
            BilateralFilter filter = new BilateralFilter(SpatialSigma, RangeSigma, false);
            ImageFrameDepth filtered = filter.Apply(image);

            // Check the result
            Assert.AreEqual(image.Width, filtered.Width);
            Assert.AreEqual(image.Height, filtered.Height);
            for (int y = 0; y < filtered.Height; y++)
            {
                for (int x = 0; x < filtered.Width; x++)
                {
                    Assert.AreEqual(filtered[x, y], 0);
                }
            }
        }

        /// <summary>
        /// Validation of bilateral filter on image of all ones
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void BilateralFilterShouldWorkForImageOfAllOnes()
        {
            const double SpatialSigma = 1.0;
            const double RangeSigma = 1.0;
            const short Value = 1000;

            // Initialize image
            ImageFrameDepth image = new ImageFrameDepth(20, 20);
            for (int y = 0; y < image.Height; y++)
            {
                for (int x = 0; x < image.Width; x++)
                {
                    image[x, y] = Value;
                }
            }

            // Perform bilateral filtering
            BilateralFilter filter = new BilateralFilter(SpatialSigma, RangeSigma, false);
            ImageFrameDepth filtered = filter.Apply(image);

            // Check the result
            Assert.AreEqual(image.Width, filtered.Width);
            Assert.AreEqual(image.Height, filtered.Height);
            const int Border = 2;
            for (int y = Border; y < filtered.Height - Border; y++)
            {
                for (int x = Border; x < filtered.Width - Border; x++)
                {
                    Assert.AreEqual(filtered[x, y], Value);
                }
            }
        }

        /// <summary>
        /// Validation of bilateral filter on image of all ones, with automatic calculation of the range sigma
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void BilateralFilterShouldWorkForImageOfAllOnesWithAutoRangeSigma()
        {
            const double SpatialSigma = 1.0;
            const double RangeSigma = 1.0;
            const short Value = 1000;

            // Initialize image
            ImageFrameDepth image = new ImageFrameDepth(20, 20);
            for (int y = 0; y < image.Height; y++)
            {
                for (int x = 0; x < image.Width; x++)
                {
                    image[x, y] = Value;
                }
            }

            // Perform bilateral filtering
            BilateralFilter filter = new BilateralFilter(SpatialSigma, RangeSigma, true);
            ImageFrameDepth filtered = filter.Apply(image);

            // Check the result
            Assert.AreEqual(image.Width, filtered.Width);
            Assert.AreEqual(image.Height, filtered.Height);
            const int Border = 2;
            for (int y = Border; y < filtered.Height - Border; y++)
            {
                for (int x = Border; x < filtered.Width - Border; x++)
                {
                    Assert.AreEqual(filtered[x, y], Value);
                }
            }
        }

        /// <summary>
        /// Validation of bilateral filter automatic calculation of the range sigma
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void BilateralFilterAutoRangeSigmaShouldComputeCorrectValue()
        {
            const double NominalDepth = 1000.0;  // in millimeters
            const double NominalRangeSigma = 10.0;  // in millimeters
            double rangeSigma;

            rangeSigma = BilateralFilter.AutoComputeRangeSigma(1000, 1 / NominalDepth, NominalRangeSigma);
            Assert.IsTrue(Math.Abs(rangeSigma - 10.0) < MathConstants.ErrorEpsilon);

            rangeSigma = BilateralFilter.AutoComputeRangeSigma(2000, 1 / NominalDepth, NominalRangeSigma);
            Assert.IsTrue(Math.Abs(rangeSigma - 20.0) < MathConstants.ErrorEpsilon);

            rangeSigma = BilateralFilter.AutoComputeRangeSigma(5000, 1 / NominalDepth, NominalRangeSigma);
            Assert.IsTrue(Math.Abs(rangeSigma - 170.0) < MathConstants.ErrorEpsilon);

            rangeSigma = BilateralFilter.AutoComputeRangeSigma(10000, 1 / NominalDepth, NominalRangeSigma);
            Assert.IsTrue(Math.Abs(rangeSigma - 820.0) < MathConstants.ErrorEpsilon);
        }

        /// <summary>
        /// Test line merging
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ImageRectLineTest()
        {
            string filename = @"\\robofile\Baker\Shared\Users\roboim\UnitTestData\Vision\RectImage.jpg";
            ImageFrameRgb image = ImageOperations2DInterop.LoadImageRgbFromFile(filename);
            List<ImageLine> lineSegments = ImageOperations2DInterop.GetLines(image);
            Assert.AreEqual(lineSegments.Count, 15);
            List<MergedLines> merged = MergedLines.DoLineMerge(lineSegments, 22.5, 80, 0.15, 5, 22);
            Assert.AreEqual(merged.Count, 19);

            // Check for an expected merged line
            merged.Sort((a, b) => { return -a.TotalLength.CompareTo(b.TotalLength); });

            bool found = false;
            int idx = 0;

            for (idx = 0; idx < merged.Count; ++idx)
            {
                if (Math.Abs(merged[idx].TotalLength - 266) < 2.0)
                {
                    found = true;
                    break;
                }
            }

            Assert.IsTrue(true == found, "Did not find expected merged line");

            MergedLines m = merged[idx];
            
            // Start and end can be interchanged
            Point2D<double> start;
            Point2D<double> end;
            if (Math.Abs(m.Start.X - 463.5) < 1.0)
            {
                start = m.Start;
                end = m.End;
            }
            else
            {
                start = m.End;
                end = m.Start;
            }

            Assert.AreEqual(start.X, 463.5, 1, "Merged start point X");
            Assert.AreEqual(start.Y, 140.4, 1, "Merged start point Y");
            Assert.AreEqual(end.X, 197.5, 1, "Merged end point X");
            Assert.AreEqual(end.Y, 150.5, 1, "Merged end point Y");

            List<ImageRectangle> rects = ImageRectangle.FindRectangles(lineSegments, 22.5, 80, 0.15, 5);
            Assert.AreEqual(rects.Count, 1);

            ImageRectangle r = rects[0];
            Assert.AreEqual(r.CenterX, 337.3, 1, "rectangle center X");
            Assert.AreEqual(r.CenterY, 234.9, 1, "rectangle center Y");
            Assert.AreEqual(r.MajorLength, 273.8, 1, "rectangle major length");
            Assert.AreEqual(r.MinorLength, 181.5, 1, "rectangle minor length");
            Assert.AreEqual(r.MajorSideAngle, 176.2, 1, "rectangle major angle");
            Assert.AreEqual(r.MinorSideAngle, 89.8, 1, "rectangle minor angle");
            double l1 = r.MajorLength / 2.0;
            double l2 = r.MinorLength / 2.0;
            double l = Math.Sqrt(l1 * l1 + l2 * l2);
            Assert.AreEqual(r.Size1D, l, 0.1, "rectangle size");
        }
        
        /// <summary>
        /// simple test for concatenating images
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ImageOps2DConcatenateImages()
        {
            // Output image will be of size 15 x 25
            Rectangle[,] boundaries = new Rectangle[,] { { new Rectangle(10, 15, 5, 10), new Rectangle(5, 7, 5, 15) }, { new Rectangle(10, 10, 10, 10), new Rectangle(2, 2, 10, 15) } };
            ImageFrame<ushort>[,] images = new ImageFrame<ushort>[2, 2];

            for (int i = 0; i < 2; ++i)
            {
                for (int j = 0; j < 2; ++j)
                {
                    images[i, j] = new ImageFrame<ushort>(30, 30);
                }
            }

            images[0, 0][10, 15] = 10;
            images[0, 1][5, 7] = 20;
            images[1, 0][10, 10] = 30;
            images[1, 1][2, 2] = 40;
            ImageFrame<ushort> image = ImageOperations2D.ConcatenateImages<ushort>(boundaries, images);

            Assert.AreEqual(image.Width, 15);
            Assert.AreEqual(image.Height, 25);
            Assert.AreEqual(image[0, 0], 10);
            Assert.AreEqual(image[5, 0], 30);
            Assert.AreEqual(image[0, 10], 20);
            Assert.AreEqual(image[5, 10], 40);
        }

        /// <summary>
        /// Test sorting by pose
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ImageOps2DSortByPose()
        {
            Dictionary<string, Pose> poseDict = new Dictionary<string, Pose>();
            Random rand = new Random(20);
            int countX = 4;
            int countY = 3;

            int idx = 0;
            double maxX = 0;
            double minX = 1.0;
            double minY = 1.0;
            double[] yVals = new double[countY];
            for (int iy = 0; iy < countY; ++iy)
            {
                double y = rand.NextDouble();
                minY = Math.Min(y, minY);
                yVals[iy] = y;
            }

            for (int ix = 0; ix < countX; ++ix)
            {
                double x = rand.NextDouble();
                maxX = Math.Max(maxX, x);
                minX = Math.Min(minX, x);

                for (int iy = 0; iy < countY; ++iy)
                {
                    double y = yVals[iy];
                    poseDict[idx.ToString()] = new Pose(new Vector3(x, y, rand.NextDouble()), new Quaternion());
                    ++idx;
                }
            }

            List<string> sortedList = PoseOperations2D.SortPoseDictByRowOrder(poseDict);

            bool isGrid = PoseOperations2D.IsPoseListGrid(poseDict, sortedList, countX, countY, 0.1);
            Assert.IsTrue(isGrid, "Grid check failed");

            // This one is a minor deviation from the grid will also pass
            Pose p = poseDict[sortedList[2]];
            poseDict[sortedList[2]] = new Pose(new Vector3(p.Position.X * 1.001, p.Position.Y, p.Position.Z));
            isGrid = PoseOperations2D.IsPoseListGrid(poseDict, sortedList, countX, countY, 0.1);
            Assert.IsTrue(isGrid, "Grid check failed");

            // Major deviation from grid should fail
            poseDict[sortedList[2]] = new Pose(new Vector3(p.Position.X * 1.2, p.Position.Y, p.Position.Z));
            isGrid = PoseOperations2D.IsPoseListGrid(poseDict, sortedList, countX, countY, 0.1);

            Assert.IsFalse(isGrid, "Grid check failed");
        }

        /// <summary>
        /// Test illegal horizontal boundary specification
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void ImageOps2DConcatenateImagesIllegalHorizontalSpec()
        {
            // Width are inconsistent
            Rectangle[,] boundaries = new Rectangle[,] { { new Rectangle(10, 15, 5, 10), new Rectangle(5, 7, 5, 15) }, { new Rectangle(10, 10, 10, 10), new Rectangle(2, 2, 12, 15) } };
            ImageFrame<ushort>[,] images = new ImageFrame<ushort>[2, 2];

            for (int i = 0; i < 2; ++i)
            {
                for (int j = 0; j < 2; ++j)
                {
                    images[i, j] = new ImageFrame<ushort>(30, 30);
                }
            }

            ImageFrame<ushort> image = ImageOperations2D.ConcatenateImages<ushort>(boundaries, images);
        }

        /// <summary>
        /// Test illegal vertical boundary specification
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void ImageOps2DConcatenateImagesIllegalVerticalSpec()
        {
            // Width are inconsistent
            Rectangle[,] boundaries = new Rectangle[,] { { new Rectangle(10, 15, 5, 10), new Rectangle(5, 7, 5, 15) }, { new Rectangle(10, 10, 10, 8), new Rectangle(2, 2, 10, 15) } };
            ImageFrame<ushort>[,] images = new ImageFrame<ushort>[2, 2];

            for (int i = 0; i < 2; ++i)
            {
                for (int j = 0; j < 2; ++j)
                {
                    images[i, j] = new ImageFrame<ushort>(30, 30);
                }
            }

            ImageFrame<ushort> image = ImageOperations2D.ConcatenateImages<ushort>(boundaries, images);
        }

        /// <summary>
        /// simple test for concatenating images
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ImageOps2DBinarizeRGB()
        {
            ImageFrameRgb input = new ImageFrameRgb(10, 5);
            int len = 10 * 5;
            Random rand = new Random(20);
            byte[] buf = new byte[3];

            for (int i = 0; i < len; ++i)
            {
                rand.NextBytes(buf);
                RgbPix pix = new RgbPix(buf[0], buf[1], buf[2]);
                input[i] = pix;
            }

            int threshSuggest = 120;
            ImageFrameRgb convertSuggest = ImageOperations2D.BinarizeRGB(input, threshSuggest);
            int thresh = ImageOperations2D.BestRGBThreshold(input, threshSuggest);
            ImageFrameRgb convert = ImageOperations2D.BinarizeRGB(input, thresh);

            RgbPix black = new RgbPix(0, 0, 0);
            RgbPix white = new RgbPix(byte.MaxValue, byte.MaxValue, byte.MaxValue);
            for (int i = 0; i < len; ++i)
            {
                RgbPix origPix = input[i];
                int v = (origPix.Red + origPix.Green + origPix.Blue) / 3;
                RgbPix convertPix = convert[i];
                RgbPix target;

                if (v > thresh)
                {
                    target = white;
                }
                else
                {
                    target = black;
                }

                Assert.AreEqual(convertPix.Red, target.Red);
                Assert.AreEqual(convertPix.Green, target.Green);
                Assert.AreEqual(convertPix.Blue, target.Blue);

                // Repeat for suggested threshold
                convertPix = convertSuggest[i];

                if (v > threshSuggest)
                {
                    target = white;
                }
                else
                {
                    target = black;
                }

                Assert.AreEqual(convertPix.Red, target.Red);
                Assert.AreEqual(convertPix.Green, target.Green);
                Assert.AreEqual(convertPix.Blue, target.Blue);
            }
        }
    }
}
